/*
 *  init.cpp
 *  FaceTracker
 *
 *  Created by Kenn Sebesta on 2007-11-22.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "ballFinder.h"
#include "cameraSetup.h"
#include "dLQRSolver.h"
#include "init.h"
#include "maskGenerate.h"
#include <OpenCV/OpenCV.h>
#include <iostream>


const char  *WINDOW_INIT  = "Ball and Plate";

void PrintMat( CvMat* );
CvMat* DLQRSolver( CvMat*, CvMat*, CvMat*, CvMat* );


void init()
{
	//====================================================================================
	// Read from file
	//====================================================================================

	// Generate hash
	// GENERATE A HASH OF FILE SO WE CAN BE SURE THE DATA IS GOOD.

	double ballDia;		// Ball diameter in [m]
	double calibrationGuess;	// Calibration of camera image in [px/m]
	double delT; // Sampling rate in [s]
	int threshLow, threshHigh; // Thresholds for the Canny edge detection.
	CvMat *actuator_1; // (x,y) location of actuator 1
	CvMat *actuator_2; // (x,y) location of actuator 2
	
	//Initialize Matrices
	CvMat *C, *Q_kalman, *R_kalman, *Q_dlqr, *R_dlqr;
	
	// Read user config file
	CvFileStorage* userVariables = cvOpenFileStorage( "bandpUserVariables.yml", 0, CV_STORAGE_READ);	
	delT = cvReadRealByName( userVariables, NULL, "delT");
	actuator_1=(CvMat*)cvReadByName( userVariables, NULL, "actuator_1" );
	actuator_2=(CvMat*)cvReadByName( userVariables, NULL, "actuator_2" );
	Q_kalman=(CvMat*)cvReadByName( userVariables, NULL, "Q_kalman" );
	R_kalman=(CvMat*)cvReadByName( userVariables, NULL, "R_kalman" );
	Q_dlqr=(CvMat*)cvReadByName( userVariables, NULL, "Q_dlqr" );
	R_dlqr=(CvMat*)cvReadByName( userVariables, NULL, "R_dlqr" );
	ballDia=cvReadRealByName( userVariables, NULL, "ballDia");
	calibrationGuess=cvReadRealByName( userVariables, NULL, "calibrationGuess");
	threshHigh = cvReadIntByName ( userVariables, NULL, "threshHigh");
	threshLow = cvReadIntByName ( userVariables, NULL, "threshLow");
	
	// Stupidity check	
	std::cout << "threshHigh is  " << threshHigh << "\n";
	std::cout << "threshLow is  " << threshLow << "\n";
	std::cout << "calibrationGuess is  " << calibrationGuess << "\n";
	printf("\nActuator 1="); PrintMat( actuator_1 );
	printf("\nActuator 2="); PrintMat( actuator_2 );
	printf("\nQ_kalman="); PrintMat( Q_kalman );
	printf("\nR_kalman="); PrintMat( R_kalman );
	printf("\nQ_dlqr="); PrintMat( Q_dlqr );
	printf("\nR_dlqr="); PrintMat( R_dlqr );

	// Local Garbage collection
	cvReleaseFileStorage( &userVariables );

	// RIGHT NOW, THERE IS NO FILE READING FOR THE BELOW.
	C=cvCreateMat(2,4,CV_32F);	cvZero(C);	cvSetReal2D( C, 0, 0, 1 );	cvSetReal2D( C, 1, 2, 1);

	printf("\nDone with file read.\n\n");	

	
	//=====================================================================================
	// Initialize camera.
	//=====================================================================================
	cvNamedWindow( WINDOW_INIT, CV_WINDOW_AUTOSIZE );
	CvCapture *camFeed = cameraSetup( );
	
	IplImage *camFrame = cvQueryFrame( camFeed ); //DO NOT RELEASE THIS IMAGE
	if ( !camFrame ) {
		fprintf(stderr, "ERROR: camFrame is null. Exiting...\n");
		exit (1);
	}
	
	
	// Loads sample image (for when no camera is present)
	//	IplImage* camFrame=cvLoadImage( "eightball.tiff", 3);
	//	if(!camFrame) printf("Could not load image file.\n"), exit(1);
	
		
	//=====================================================================================
	// Camera adjustment. MESSSY SECTION. NEEDS TO BE IMPROVED.
	//=====================================================================================

	// Setup
	char wndname[] = "Live Edge Detection";
	char tbarname1[] = "Upper Threshold";
	char tbarname2[] = "Lower Threshold";

	IplImage *edgeImage;
	edgeImage=cvCreateImage( cvSize(camFrame->width, camFrame->height), camFrame->depth, 1 );
	IplImage *grayImage;
	grayImage=cvCreateImage( cvSize(camFrame->width, camFrame->height), camFrame->depth, 1 );
	cvNamedWindow(wndname, 1);

	// create a toolbar
	cvCreateTrackbar(tbarname1, wndname, &threshHigh, 300, NULL );
	cvCreateTrackbar(tbarname2, wndname, &threshLow, 50, NULL );

	// Find best threshold value
	while(cvWaitKey(1)!= 27)
	{
		camFrame = cvQueryFrame( camFeed );
		cvSplit( camFrame, grayImage, NULL, NULL, NULL );

		// Run the edge detector on grayscale
		cvCanny(grayImage, edgeImage, (float)threshLow, (float)threshHigh, 3);

		// Show the image
		cvShowImage(wndname, edgeImage);
	}
	
	cvDestroyWindow( wndname );
	
	printf("Done setting up camera. The threshold values are %d and %d.\n\n", threshHigh, threshLow);	

	
	//=====================================================================================
	// Auto-calibration and maskImage generation
	//=====================================================================================
	double oldMaxConv=0, maxConv=0, peakConv=0, *convReturn, bestCalibration=calibrationGuess;
	convReturn=&maxConv;
	CvRect cropRect;
	CvMat *y_measrd = cvCreateMat( 2, 1, CV_32F );	
	
	IplImage *bwImageOUT, *edgeImageOUT;
	CvMat *convMatOUT;


	//Auto-calibration loop. PRETTY BOLLOXED UP. NEEDS TO BE RETHOUGHT.
	int count=0;
	double step=1; // Decreases calibration by "step" [px/m] by in each iteration.
	while(peakConv - maxConv < 1e06)
	{
		count++;
		if (count > 100){
			fprintf(stderr, "ERROR: cannot find proper calibration. Continuing...\n"); 
			break;
		}
		oldMaxConv=maxConv;
		calibrationGuess-=step;
		cropRect=cvRect(0,0,0,0);
		
		// Creates ball mask
		IplImage* maskImage;
		maskImage = maskGenerate( ballDia, calibrationGuess );
		ballFinder ( maskImage, camFrame, cropRect, threshLow, threshHigh,	//Inputs
						NULL, convReturn, y_measrd, &convMatOUT, &edgeImageOUT, &bwImageOUT );		//Outputs
		
		// Tests fit
		if (peakConv < maxConv) 
			peakConv=maxConv, bestCalibration = calibrationGuess;	
		
		// Show results
		cvShowImage( WINDOW_INIT, maskImage );
		cvWaitKey(10);
		cvReleaseImage( &maskImage );	// Local garbage collection
	}
	
	IplImage* maskImage = maskGenerate( ballDia, bestCalibration );
	std::cout << "Best calibration value is " << bestCalibration << std::endl;
	cvCircle( camFrame, cvPoint(round(cvmGet(y_measrd, 0, 0)), round(cvmGet(y_measrd, 1, 0))), (maskImage->width-1)/2, cvScalar(255, 255, 255), 1 );	
	cvShowImage( WINDOW_INIT, camFrame );
	cvWaitKey(1200);
	
	printf("Done with auto-calibration.\n\n");	

	
	//====================================================================================
	// Matrix Generation
	//====================================================================================
	CvMat *A, *B, *expNthPower, *expMatA, *expMatB, *expMat;
	CvMat *A_dscrt, *B_dscrt;
	double x1=cvmGet( actuator_1, 0, 0 );	double y1=cvmGet( actuator_1, 0, 1 );
	double x2=cvmGet( actuator_2, 0, 0 );	double y2=cvmGet( actuator_2, 0, 1 );
	double kt=5.0/7.0, g=9.81, Nz=x1*y2-x2*y1; // Physical constants

	// Initialize matrices
	A=cvCreateMat( 4, 4, CV_32F );
	B=cvCreateMat( 4, 2, CV_32F );
	A_dscrt=cvCreateMat(4,4,CV_32F);
	B_dscrt=cvCreateMat(4,2,CV_32F);
	expNthPower=cvCreateMat( A->width+B->width, A->height+B->width, CV_32F );
	expMat=cvCreateMat( A->width+B->width, A->height+B->width, CV_32F );
	expMatA=cvCreateMat( A->width, A->height, CV_32F );
	expMatB=cvCreateMat( B->width, B->height, CV_32F );

	// Fill matrices
	cvZero( A ); cvZero( B ); 	cvZero( expNthPower );
	cvmSet( A, 0, 1, 1); cvmSet( A, 2, 3, 1);
	cvmSet( B, 1, 0, kt*g/Nz*(-y2)); cvmSet( B, 1, 1, kt*g/Nz*(y1)); cvmSet( B, 3, 0, kt*g/Nz*(x2)); cvmSet( B, 3, 1, kt*g/Nz*(-x1));
	cvSetIdentity( expMat, cvScalar(1) );
	cvGetSubRect( expNthPower, expMatA, cvRect( 0, 0, A->width, A->height ) );
	cvGetSubRect( expNthPower, expMatB, cvRect( A->width, 0, B->width, B->height ) );
	cvCopy( A, expMatA );
	cvCopy( B, expMatB );
	cvScale( expNthPower, expNthPower, delT );
	
	// Calculate e^[expMat].
	count=0;	
	cvAdd( expMat, expNthPower, expMat );
	while( cvNorm( expNthPower ) > 0.0001)
	{
		count++;
		if (count > 100){
			fprintf(stderr, "ERROR: Linear matrices do not converge to discrtized matrices...\n"); 
			exit(1);
		}
		cvMatMul( expNthPower, expNthPower, expNthPower );
		cvAddWeighted(expMat, 1, expNthPower, 1.0/(double)count, 0, expMat );
	}
	
	// Extract discretized matrices
	cvGetSubRect( expMat, expMatA, cvRect( 0, 0, A->width, A->height ) );
	cvGetSubRect( expMat, expMatB, cvRect( A->width, 0, B->width, B->height ) );
	cvCopy( expMatA, A_dscrt );
	cvCopy( expMatB, B_dscrt );
	
	printf("\nA_dscrt="); PrintMat(A_dscrt);
	printf("\nB_dscrt="); PrintMat(B_dscrt);
	printf("\nC="); PrintMat(C);	
	
	//Local garbage collection
	cvReleaseMat( &A );
	cvReleaseMat( &B );
	cvReleaseMat( &expNthPower );
	cvReleaseMat( &expMat );
	cvReleaseMat( &expMatA );
	cvReleaseMat( &expMatB );
			
	printf("Done with matrix generation.\n");
	
	
	//====================================================================================
	// Initialize Kalman filter error covariance matrix.
	//====================================================================================
	CvKalman *kalman_dscrt;
		kalman_dscrt = cvCreateKalman(4,2,2);
	CvMat *oldPkCovMat;
		oldPkCovMat=cvCreateMat(4,4,CV_32F);
	CvMat *y_measrdR = cvCreateMat( 2, 1, CV_32F );	
		cvScale( y_measrd, y_measrdR, 1/bestCalibration );
	
	cvCopy(A_dscrt, kalman_dscrt->transition_matrix);						// A Matrix 
	cvCopy(B_dscrt, kalman_dscrt->control_matrix);							// B Matrix
	cvCopy(C, kalman_dscrt->measurement_matrix);								// C Matrix	
	cvCopy(Q_kalman, kalman_dscrt->process_noise_cov);						// Q Matrix	
	cvCopy(R_kalman, kalman_dscrt->measurement_noise_cov);				// R Matrix	

	
	// Repeats until the error covariance matrix converges.
	double diffNorm=1;
	while (  diffNorm> 0.000001 ){
		cvCopy( kalman_dscrt->error_cov_post, oldPkCovMat ); // Save old error covariance matrix
		cvKalmanPredict( kalman_dscrt, 0 );
		cvKalmanCorrect( kalman_dscrt, y_measrdR );
		diffNorm=cvNorm( oldPkCovMat, kalman_dscrt->error_cov_post ); // Are we converged yet?
	}
	
	std::cout << std::endl << "The error covariance matrix is:";	PrintMat( kalman_dscrt->error_cov_post );
	
	// Local garbage collection
	cvReleaseMat(&oldPkCovMat);
	
	printf("\nDone with Kalman filter initialization.\n");	
	
	
	//=====================================================================================
	// Generate DLQR Solver
	//=====================================================================================
	CvMat *K_dlqr;
	K_dlqr=dLQRSolver( A_dscrt, B_dscrt, Q_dlqr, R_dlqr );
	std::cout << std::endl<< "The dLQR matrix is:";	PrintMat( K_dlqr );
	
	// Local garbage collection
	cvReleaseMat( &Q_dlqr );
	cvReleaseMat( &R_dlqr );
	
	printf("\nDone with DLQR Solver.\n\n");	

	
	//=====================================================================================
	// Write matrices to file
	//=====================================================================================
	CvFileStorage* systemVariables = cvOpenFileStorage( "bandpSystemVariables.yml", 0, CV_STORAGE_WRITE);
	cvWriteInt( systemVariables, "threshHigh", threshHigh );
	cvWriteInt( systemVariables, "threshLow", threshLow );
	cvWriteReal( systemVariables, "calibration", bestCalibration );
	cvWrite( systemVariables, "A_dscrt", A_dscrt );
	cvWrite( systemVariables, "B_dscrt", B_dscrt );
	cvWrite( systemVariables, "C", C );
	cvWrite( systemVariables, "x_hatR", kalman_dscrt->state_post );
	cvWrite( systemVariables, "PkCovMat", kalman_dscrt->error_cov_post );
	cvWrite( systemVariables, "K_dlqr", K_dlqr );
	cvWrite( systemVariables, "maskImage", maskImage );
	cvReleaseFileStorage( &systemVariables );
	
	printf("Done with file write.\n\n");	


	//=====================================================================================
	// Garbage collection
	//=====================================================================================
	cvReleaseMat( &A_dscrt );
	cvReleaseMat( &B_dscrt );
	cvReleaseMat( &C );
	cvReleaseMat( &K_dlqr );
	cvReleaseMat( &y_measrd );
	cvReleaseMat( &y_measrdR );
	cvReleaseImage( &maskImage );
//	cvReleaseImage( &camFrame );
	cvReleaseKalman( &kalman_dscrt );
	cvDestroyWindow( WINDOW_INIT );
	cvReleaseCapture( &camFeed );
	return;
}

//=====================================
void PrintMat(CvMat *A)
{
	int i, j;
	for (i = 0; i < A->rows; i++)
	{
		printf("\n"); 
		switch (CV_MAT_DEPTH(A->type))
		{
			case CV_32F:
			case CV_64F:
				for (j = 0; j < A->cols; j++)
					printf ("%8.3f ", (float)cvGetReal2D(A, i, j));
				break;
				case CV_8U:
				case CV_16U:
				for(j = 0; j < A->cols; j++)
					printf ("%6d",(int)cvGetReal2D(A, i, j));
				break;
				default:
				break;
		}
	}
	printf("\n");
}